#include <algorithm>
#include <iostream>
#include <atomic>
#include <array>
#include <iomanip>
#include <random>

#include "CanDriver.hpp"
#include "src/PID.h"

using sockcanpp::CanDriver;
using sockcanpp::CanMessage;
using std::atomic;
using std::array;

can_frame control_frame;

PID speed_pid;
std::atomic<float> amp_output = 0.0f;



[[noreturn]] void readFeedback(CanDriver& canDriver) {
    while (true) {
        if (canDriver.waitForMessages()) {
            const auto feedback = canDriver.readMessage().getRawFrame();
            uint16_t p_int = static_cast<uint16_t>(feedback.data[0] << 8) | feedback.data[1];
            int16_t v_int = static_cast<int16_t>(feedback.data[2] << 8) | feedback.data[3];
            int16_t a_int = static_cast<int16_t>(feedback.data[4] << 8) | feedback.data[5];
            uint8_t temperature = feedback.data[6];
             if (std::abs(a_int) > 15000 || temperature > 80) {
                 throw std::runtime_error("Temperature or electric have reached the limited.");
             }
            amp_output = PIDIncControl(&speed_pid, v_int);
        }
    }
}

void getUserInput() {
    int input;
    while (true) {
        std::cin >> input;
        if (input == 0 || input == 1 || input == 2 || input == 3) {
            if (input == 0) break;
        } else {
            std::cout << "Invalid input. Please enter 0, 1, 2 or 3." << std::endl;
        }
    }
}


[[noreturn]] void sendElectric(CanDriver& canDriver) {
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    while (true) {
        const auto output = static_cast<int16_t>(amp_output.load(std::memory_order_acquire));
	//std::cout << output <<std::endl;
        //if (std::abs(output) >= 12000) continue; // limit
        uint8_t data_array[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
        data_array[0] = output >> 8;
        data_array[1] = output & 0xff;
        std::copy_n(data_array, 8, control_frame.data);
        canDriver.sendMessage(control_frame);
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

int main() {
    speed_pid.Ref = -15.0;

    control_frame.can_id = 0x200;
    control_frame.len = 8;
    array<uint8_t, 8> init_data = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    std::copy_n(init_data.begin(), 8, control_frame.data);

    CanDriver controlDriver("can0", CAN_RAW);
    CanDriver readDriver("can0", CAN_RAW);
    readDriver.setCanFilterMask(0x7FF, 0x201);

    std::thread reader(readFeedback, std::ref(readDriver));
    std::thread sender(sendElectric, std::ref(controlDriver));
    reader.join();
    sender.join();

    return 0;
}
